#include <ros/ros.h>
#include <tf2/utils.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>

#include <gflags/gflags.h>
#include <glog/logging.h>

#include <mc_common/base_define.h>
#include <mc_velocity_planning/velocity_planning_base.h>
#include <mc_velocity_planning/dwa/dwa.h>
#include <mc_velocity_planning/move_base/move_pose.h>
#include <mc_velocity_planning/pure_suit/pure_suit.h>
#include <mc_debug/tracked_path.h>

class TestMoveBase{
    public:
        TestMoveBase(tf2_ros::Buffer* tf_buffer);
        // Rviz位姿回调函数
        void RvizPosesCallback(const geometry_msgs::PoseStamped::Ptr& msg);    

         // 循环控制
        void Loop();      

    private:
        ros::NodeHandle nh_;
        tf2_ros::Buffer* const tfBuffer_;
        ros::Subscriber rvizPoseSub_;
        ros::Publisher localGoalPosePub_;
        ros::Publisher trackedPathPub_;
        boost::shared_ptr<DWA> velocityPlannerPtr_;
        boost::shared_ptr<geometry_msgs::PoseStamped> rvizGoalPtr_;
        TrackedPath trackedPath_;

        boost::shared_ptr<MovePose> movePosePtr_;
        boost::shared_ptr<PureSuit> pureSuitPtr_;
        
};


TestMoveBase::TestMoveBase(tf2_ros::Buffer* tf_buffer):tfBuffer_(tf_buffer),rvizGoalPtr_(nullptr){
    movePosePtr_ = boost::make_shared<MovePose>(); 
    pureSuitPtr_ = boost::make_shared<PureSuit>();
    velocityPlannerPtr_ = boost::make_shared<DWA>(tf_buffer);
    localGoalPosePub_ = nh_.advertise<geometry_msgs::PoseStamped>("/local_goal_pose", 10);
    trackedPathPub_ = nh_.advertise<nav_msgs::Path>("/tracked_path", 1);
    rvizPoseSub_ = nh_.subscribe("/move_base_simple/goal", 10, &TestMoveBase::RvizPosesCallback, this);  

    // 循环控制
    Loop();
}

// Rviz位姿回调函数
void TestMoveBase::RvizPosesCallback(const geometry_msgs::PoseStamped::Ptr& msg){
    LOG(INFO) << "[RvizPosesCallback] Receive a pose. x=" << msg->pose.position.x
        << ", y=" << msg->pose.position.y
        << ", yaw=" << tf2::getYaw(msg->pose.orientation)
        << ", frame_id=" << msg->header.frame_id;
    rvizGoalPtr_ = msg;
    localGoalPosePub_.publish(*msg);
}


// 循环控制
void TestMoveBase::Loop(){
    ros::Rate controller_rate(20);  //20Hz
    while(!ros::isShuttingDown()){
        controller_rate.sleep();
        ros::spinOnce();
        LOG(INFO) << "--------------------------------";

        if(!rvizGoalPtr_) continue;
        
        // 更新机器人当前位姿
        geometry_msgs::PoseStamped robotPoseStamp;
        if(!velocityPlannerPtr_->GetRobotPose(robotPoseStamp)){
            LOG(WARNING) << "can not get robot pose!";
            continue;
        }
        Pose2D robotPose2dGlobal = msg_convert::ToPose2D(robotPoseStamp.pose);
        cout << "[Loop] robotPose2dGlobal: " << msg_convert::Pose2D2Str(robotPose2dGlobal) << endl;

        // 发布运动轨迹
        trackedPath_.addPoseStamped(robotPoseStamp);
        trackedPathPub_.publish(trackedPath_.toPath("map"));

        Velocity2D curr_vel;
        Velocity2D next_vel;
        Pose2D goalPose2dGlobal = msg_convert::ToPose2D(rvizGoalPtr_->pose);
        Pose2D goalPose2dLocal = math_utils::Pose2DGlobal2Local(robotPose2dGlobal, goalPose2dGlobal);
        cout << "[Loop] local_goal: " << msg_convert::Pose2D2Str(goalPose2dLocal) << endl;
        // 简单的PID控制测试
        movePosePtr_->ComputeTwist(curr_vel, next_vel, goalPose2dLocal);
        // 简单的纯跟踪控制测试
        // pureSuitPtr_->ComputeTwist(curr_vel, next_vel, goalPose2dLocal);
        cout << "[Loop] next_vel: " << msg_convert::Velocity2D2Str(next_vel) << endl;

        // 判断是否到达终点
        if(velocityPlannerPtr_->IsPositionReached(robotPose2dGlobal, goalPose2dGlobal, 0.05)){
            ROS_INFO("The robot reaches the goal point.");
            velocityPlannerPtr_->PublishEmptyTwist();
            // ros::shutdown();
        }
        else{
            // 速度指令控制
            velocityPlannerPtr_->PublishTwist(next_vel);
        }
    }
}


int main(int argc, char** argv){
    FLAGS_alsologtostderr = true;           // 日志同时输出到stderr
    FLAGS_max_log_size = 1;                 // 单个日志文件大小上限（MB）, 如果设置为0将默认为1
	FLAGS_stop_logging_if_full_disk = true; // 当磁盘写满时，停止输出
    FLAGS_minloglevel = google::INFO;    // 只记录大于{}等级的日志
    // FLAGS_log_dir = "/home/xxty/user_config/logs";
    google::InitGoogleLogging(argv[0]);

    ros::init(argc, argv, "test_move_pose");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    ROS_INFO("test_move_pose start...");
    tf2_ros::Buffer tfBuffer(::ros::Duration(10));
    tf2_ros::TransformListener tfListener(tfBuffer);
    TestMoveBase test_move_pose(&tfBuffer);
    ros::spin();

    google::ShutdownGoogleLogging();

    return 0;
}
